#include "analyzer_common.h"

void dblas_3x3_gemm_a_eq_axb(register double *ap0, register double *bp0)
{
    register double rg0, rg1, rg2;

    register double ax0, ax1, ax2;
    register double b00, b01, b02;
    register double b10, b11, b12;
    register double b20, b21, b22;

    register double *ap1 = ap0 + 1, *ap2 = ap0 + 2;
    register double *bp1 = bp0 + 3, *bp2 = bp0 + 6;

    ax0 = *ap0, ax1 = *ap1, ax2 = *ap2;
    b00 = *bp0, b10 = *bp1, b20 = *bp2;

    rg1 = ax0 * b00;
    rg0 = ax1 * b10;
    rg1 += ax2 * b20;
    *ap0 = rg0 + rg1;

    bp0 += 1, bp1 += 1, bp2 += 1;
    b01 = *bp0, b11 = *bp1, b21 = *bp2;
    rg2 = ax0 * b01;
    rg1 = ax1 * b11;
    rg2 += ax2 * b21;
    *ap1 = rg1 + rg2;

    ap0 += 3, ap1 += 3;
    bp0 += 1, bp1 += 1, bp2 += 1;
    b02 = *bp0, b12 = *bp1, b22 = *bp2;
    rg2 = ax0 * b02;
    ax0 = *ap0;
    rg0 = ax1 * b12;
    ax1 = *ap1;
    rg2 += ax2 * b22;
    *ap2 = rg0 + rg2;
    ap2 += 3;

    rg1 = ax0 * b00;
    ax2 = *ap2;
    rg0 = ax1 * b10;
    rg1 += ax2 * b20;
    *ap0 = rg0 + rg1;

    rg2 = ax0 * b01;
    rg1 = ax1 * b11;
    rg2 += ax2 * b21;
    *ap1 = rg1 + rg2;

    ap0 += 3, ap1 += 3;
    rg2 = ax0 * b02;
    ax0 = *ap0;
    rg0 = ax1 * b12;
    ax1 = *ap1;
    rg2 += ax2 * b22;
    *ap2 = rg0 + rg2;
    ap2 += 3;

    rg1 = ax0 * b00;
    ax2 = *ap2;
    rg0 = ax1 * b10;
    rg1 += ax2 * b20;
    *ap0 = rg0 + rg1;

    rg2 = ax0 * b01;
    rg1 = ax1 * b11;
    rg2 += ax2 * b21;
    *ap1 = rg1 + rg2;

    rg2 = ax0 * b02;
    rg0 = ax1 * b12;
    rg2 += ax2 * b22;
    *ap2 = rg0 + rg2;
}

void dblas_3x3_gemm_b_eq_axb(register double *ap0, register double *bp0)
{
    register double rg0, rg1, rg2;

    register double b0x, b1x, b2x;
    register double a00, a01, a02;
    register double a10, a11, a12;
    register double a20, a21, a22;

    register double *ap1 = ap0 + 1, *ap2 = ap0 + 2;
    register double *bp1 = bp0 + 3, *bp2 = bp0 + 6;

    a00 = *ap0, a01 = *ap1, a02 = *ap2;
    b0x = *bp0, b1x = *bp1, b2x = *bp2;

    rg1 = a00 * b0x;
    rg0 = a01 * b1x;
    rg1 += a02 * b2x;
    *bp0 = rg0 + rg1;

    ap0 += 3, ap1 += 3, ap2 += 3;
    a10 = *ap0, a11 = *ap1, a12 = *ap2;
    rg2 = a10 * b0x;
    rg1 = a11 * b1x;
    rg2 += a12 * b2x;
    *bp1 = rg1 + rg2;

    bp0 += 1, bp1 += 1;
    ap0 += 3, ap1 += 3, ap2 += 3;
    a20 = *ap0, a21 = *ap1, a22 = *ap2;
    rg2 = a20 * b0x;
    b0x = *bp0;
    rg0 = a21 * b1x;
    b1x = *bp1;
    rg2 += a22 * b2x;
    *bp2 = rg0 + rg2;
    bp2 += 1;

    rg1 = a00 * b0x;
    b2x = *bp2;
    rg0 = a01 * b1x;
    rg1 += a02 * b2x;
    *bp0 = rg0 + rg1;

    rg2 = a10 * b0x;
    rg1 = a11 * b1x;
    rg2 += a12 * b2x;
    *bp1 = rg1 + rg2;

    bp0 += 1, bp1 += 1;
    rg2 = a20 * b0x;
    b0x = *bp0;
    rg0 = a21 * b1x;
    b1x = *bp1;
    rg2 += a22 * b2x;
    *bp2 = rg0 + rg2;
    bp2 += 1;

    rg1 = a00 * b0x;
    b2x = *bp2;
    rg0 = a01 * b1x;
    rg1 += a02 * b2x;
    *bp0 = rg0 + rg1;

    rg2 = a10 * b0x;
    rg1 = a11 * b1x;
    rg2 += a12 * b2x;
    *bp1 = rg1 + rg2;

    rg2 = a20 * b0x;
    rg0 = a21 * b1x;
    rg2 += a22 * b2x;
    *bp2 = rg0 + rg2;
}

// void dblas_3x3_gemm_c_minus_axb(register double *a, register double *b, register double *c)
// {
//     register double b00 = b[0], b01 = b[1], b02 = b[2];
//     register double b10 = b[3], b11 = b[4], b12 = b[5];
//     register double b20 = b[6], b21 = b[7], b22 = b[8];

//     register double a0, a1, a2;
//     register double c0, c1, c2;

//     a0 = a[0], a1 = a[1], a2 = a[2];
//     c0 = 0, c1 = 0, c2 = 0;
//     c0 += a0 * b00;
//     c1 += a0 * b01;
//     c2 += a0 * b02;
//     c0 += a1 * b10;
//     c1 += a1 * b11;
//     c2 += a1 * b12;
//     c0 += a2 * b20;
//     c1 += a2 * b21;
//     c2 += a2 * b22;
//     c[0] -= c0, c[1] -= c1, c[2] -= c2;

//     a0 = a[3], a1 = a[4], a2 = a[5];
//     c0 = 0, c1 = 0, c2 = 0;
//     c0 += a0 * b00;
//     c1 += a0 * b01;
//     c2 += a0 * b02;
//     c0 += a1 * b10;
//     c1 += a1 * b11;
//     c2 += a1 * b12;
//     c0 += a2 * b20;
//     c1 += a2 * b21;
//     c2 += a2 * b22;
//     c[3] -= c0, c[4] -= c1, c[5] -= c2;

//     a0 = a[6], a1 = a[7], a2 = a[8];
//     c0 = 0, c1 = 0, c2 = 0;
//     c0 += a0 * b00;
//     c1 += a0 * b01;
//     c2 += a0 * b02;
//     c0 += a1 * b10;
//     c1 += a1 * b11;
//     c2 += a1 * b12;
//     c0 += a2 * b20;
//     c1 += a2 * b21;
//     c2 += a2 * b22;
//     c[6] -= c0, c[7] -= c1, c[8] -= c2;
// }


void dblas_3x3_gemm_c_minus_axb(register double *a, register double *b, register double *c)
// void dblas_3x3_gemm_c_minus_axb(volatile register double *a, volatile register double *b, volatile register double *c)
{
    register double a00 = a[0], a01 = a[1], a02 = a[2];
    register double a10 = a[3], a11 = a[4], a12 = a[5];
    register double a20 = a[6], a21 = a[7], a22 = a[8];

    register double b0, b1, b2;
    register double c0, c1, c2;

    b0 = b[0], b1 = b[3], b2 = b[6];
    c0 = 0, c1 = 0, c2 = 0;
    c0 += a00 * b0;
    c1 += a10 * b0;
    c2 += a20 * b0;
    c0 += a01 * b1;
    c1 += a11 * b1;
    c2 += a21 * b1;
    c0 += a02 * b2;
    c1 += a12 * b2;
    c2 += a22 * b2;
    c[0] -= c0, c[3] -= c1, c[6] -= c2;

    b0 = b[1], b1 = b[4], b2 = b[7];
    c0 = 0, c1 = 0, c2 = 0;
    c0 += a00 * b0;
    c1 += a10 * b0;
    c2 += a20 * b0;
    c0 += a01 * b1;
    c1 += a11 * b1;
    c2 += a21 * b1;
    c0 += a02 * b2;
    c1 += a12 * b2;
    c2 += a22 * b2;
    c[1] -= c0, c[4] -= c1, c[7] -= c2;

    b0 = b[2], b1 = b[5], b2 = b[8];
    c0 = 0, c1 = 0, c2 = 0;
    c0 += a00 * b0;
    c1 += a10 * b0;
    c2 += a20 * b0;
    c0 += a01 * b1;
    c1 += a11 * b1;
    c2 += a21 * b1;
    c0 += a02 * b2;
    c1 += a12 * b2;
    c2 += a22 * b2;
    c[2] -= c0, c[5] -= c1, c[8] -= c2;
}

// register double a00, a01, a02;
// register double a10, a11, a12;
// register double a20, a21, a22;

// double dblas_inv_temp[18];

// void dblas_3x3_inv(register double *a)
// {
//     // 构造扩展矩阵[A | I]
//     for (int i = 0; i < 3; i++)
//     {
//         for (int j = 0; j < 3; j++)
//         {
//             dblas_inv_temp[i * 6 + j] = a[i * 3 + j];
//             dblas_inv_temp[i * 6 + j + 3] = (i == j) ? 1.0 : 0.0;
//         }
//     }

//     // 高斯-约当消去法
//     for (int i = 0; i < 3; i++)
//     {

//         double diagElement = dblas_inv_temp[i * 6 + i];

//         for (int j = 0; j < 6; j++)
//         {
//             dblas_inv_temp[i * 6 + j] /= diagElement;
//         }

//         for (int k = 0; k < 3; k++)
//         {

//             if (k != i)
//             {

//                 double ratio = dblas_inv_temp[k * 6 + i];

//                 for (int j = 0; j < 6; j++)
//                 {

//                     dblas_inv_temp[k * 6 + j] -= ratio * dblas_inv_temp[i * 6 + j];
//                 }
//             }
//         }
//     }

//     // 提取逆矩阵

//     for (int i = 0; i < 3; i++)
//     {

//         for (int j = 0; j < 3; j++)
//         {
//             a[i * 3 + j] = dblas_inv_temp[i * 6 + j + 3];
//         }
//     }
// }


// void dblas_3x3_inv(register double *a)
// {
//     register double r0,r1,r2,r3,r4,r5,r6,r7,r8;

//     r0 = 1/a[0];
//     a[1] *= r0;
//     a[2] *= r0;
    
//     a[4] -= a[3] * a[1];
//     a[5] -= a[3] * a[2];
//     r3 = a[3] * (-r0);

//     a[7] -= a[6] * a[1];
//     a[8] -= a[6] * a[2];
//     r6 = a[6] * (-r0);
    

//     r4 = 1/a[4];
//     r3 *= r4;
//     a[5] *= r4;

//     a[2] -= a[1] * a[5];
//     r0 -= a[1] * r3;
//     r1 = a[1] * (-r4);

//     a[8] -= a[7] * a[5];
//     r6 -= a[7] * r3;
//     r7 -= a[7] * r4;


//     r8 = 1/a[8];
//     r6 *= r8;
//     r7 *= r8;

//     r0 -= a[2] * r6;
//     r1 -= a[2] * r7;
//     r2 = a[2] * (-r8);

//     r3 -= a[5] * r6;
//     r4 -= a[5] * r7;
//     r5 = a[5] * (-r8);

//     a[0] = r0;
//     a[1] = r1;
//     a[2] = r2;
//     a[3] = r3;
//     a[4] = r4;
//     a[5] = r5;
//     a[6] = r6;
//     a[7] = r7;
//     a[8] = r8;
// }


void dblas_3x3_inv(register double *a)
{
    register double r0,r1,r2,r3,r4,r6,r7,r8,rpiv;

    r0 = 1/a[0];
    a[1] *= r0;
    a[2] *= r0;
    
    rpiv = a[3];
    a[4] -= rpiv * a[1];
    a[5] -= rpiv * a[2];
    r3 = rpiv * (-r0);

    rpiv = a[6];
    a[7] -= rpiv * a[1];
    a[8] -= rpiv * a[2];
    r6 = rpiv * (-r0);
    

    r4 = 1/a[4];
    r3 *= r4;
    a[5] *= r4;

    rpiv = a[1];
    a[2] -= rpiv * a[5];
    r0 -= rpiv * r3;
    r1 = rpiv * (-r4);

    rpiv = a[7];
    a[8] -= rpiv * a[5];
    r6 -= rpiv * r3;
    r7 -= rpiv * r4;


    r8 = 1/a[8];
    r6 *= r8;
    r7 *= r8;
    a[6] = r6;
    a[7] = r7;
    a[8] = r8;

    rpiv = a[2];
    r0 -= rpiv * r6;
    r1 -= rpiv * r7;
    r2 = rpiv * (-r8);
    a[0] = r0;
    a[1] = r1;
    a[2] = r2;

    r3 -= a[5] * r6;
    r4 -= a[5] * r7;
    a[3] = r3;
    a[4] = r4;
    a[5] *= (-r8);
}


volatile int analyzer_blas_check(){
    double* a = malloc(sizeof(double) * 9);
    double* b = malloc(sizeof(double) * 9);
    double* c = malloc(sizeof(double) * 9);
    double* tmp = malloc(sizeof(double) * 9);

    int errcnt = 0;

    for(int i=0;i<9;i++){
        a[i] = rand() * 3412.341324;
        b[i] = rand() * 3412.341324;
        c[i] = 0;
    }

    int bench_times = 1000000000;
    struct timeval time_start;
    analyzer_timer_start(&time_start);
    // #pragma unroll
    for(int i=0;i<bench_times;i++){
        dblas_3x3_gemm_c_minus_axb(a, b, c);
    }
    double time_elapsed = analyzer_timer_stop(&time_start);

    for(int i=0;i<9;i++){
        a[i] = c[i] / bench_times;
        b[i] = c[i] / bench_times;
        c[i] = 0;
        tmp[i] = 0;
    }
    for(int i=0;i<3;i++){
        for(int j=0;j<3;j++){
            for(int k=0;k<3;k++){
                tmp[i * 3 + j] -= a[i * 3 + k] * b[k * 3 + j];
            }
        }
    }
    dblas_3x3_gemm_c_minus_axb(a, b, c);

    errcnt = 0;
    for(int i=0; i<9; i++){
        if(tmp[i] != c[i]){
            errcnt++;
            printf("i=%d ref=%lf myans=%lf\n", i, tmp[i], c[i]);
        }
    }
    printf("errcnt = %d time_elapsed_avg = %.2lf ns ( ~ %.0lf ticks ), %lf Gflops\n",
        errcnt,
        time_elapsed * 1e9 / bench_times,
        (time_elapsed * 1e9 / bench_times)*5.6,
        (2 * 27.0) / (time_elapsed * 1e3 / bench_times) / 1e6
    );

    free(a);
    free(b);
    free(c);
    free(tmp);
}

// int analyzer_blas_check(){
//     double* a = malloc(sizeof(double) * 9);
//     double* b = malloc(sizeof(double) * 9);
//     double* c = malloc(sizeof(double) * 9);
//     double* tmp = malloc(sizeof(double) * 9);

//     int errcnt = 0;

//     for(int i=0;i<9;i++){
//         a[i] = (i+3)/(5.0);
//         b[i] = (i+3)/(5.0);
//         c[i] = (i+3)/(5.0);
//     }

//     int bench_times = 1000000000;
//     struct timeval time_start;
//     analyzer_timer_start(&time_start);
//     for(int i=0;i<bench_times;i++){
//         dblas_3x3_gemm_a_eq_axb(a, b);
//     }
//     double time_elapsed = analyzer_timer_stop(&time_start);

//     for(int i=0;i<9;i++){
//         a[i] += i+3;
//         b[i] += i+3;
//         c[i] += i+3;
//     }
//     memset(tmp, 0, sizeof(double) * 9);
//     for(int i=0;i<3;i++){
//         for(int j=0;j<3;j++){
//             for(int k=0; k<3;k++){
//                 tmp[i * 3 + j] += a[i * 3 + k] * b[k * 3 + j];
//             }
//         }
//     }
//     dblas_3x3_gemm_a_eq_axb(a, b);

//     errcnt = 0;
//     for(int i=0; i<9; i++){
//         if(tmp[i] != a[i]){
//             errcnt++;
//             printf("i=%d ref=%lf myans=%lf\n", i, tmp[i], a[i]);
//         }
//     }
//     printf("errcnt = %d time_elapsed_avg = %.2lf ns ( ~ %.0lf ticks ), %lf Gflops\n",
//         errcnt,
//         time_elapsed * 1e9 / bench_times,
//         (time_elapsed * 1e9 / bench_times)*5.6,
//         (2 * 27.0) / (time_elapsed * 1e3 / bench_times) / 1e6
//     );

//     free(a);
//     free(b);
//     free(c);
//     free(tmp);
// }

// int analyzer_blas_check(){
//     double* a = malloc(sizeof(double) * 9);
//     double* b = malloc(sizeof(double) * 9);
//     double* c = malloc(sizeof(double) * 9);
//     double* tmp = malloc(sizeof(double) * 9);

//     int errcnt = 0;

//     for(int i=0;i<9;i++){
//         a[i] = (i+3)/(5.0);
//         b[i] = (i+3)/(5.0);
//         c[i] = (i+3)/(5.0);
//     }

//     int bench_times = 1000000000;
//     struct timeval time_start;
//     analyzer_timer_start(&time_start);
//     for(int i=0;i<bench_times;i++){
//         dblas_3x3_gemm_b_eq_axb(a, b);
//     }
//     double time_elapsed = analyzer_timer_stop(&time_start);

//     for(int i=0;i<9;i++){
//         a[i] += (i+3.0);
//         b[i] += (i+3.0);
//         c[i] += (i+3.0);
//     }
//     memset(tmp, 0, sizeof(double) * 9);
//     for(int i=0;i<3;i++){
//         for(int j=0;j<3;j++){
//             for(int k=0; k<3;k++){
//                 tmp[i * 3 + j] += a[i * 3 + k] * b[k * 3 + j];
//             }
//         }
//     }
//     dblas_3x3_gemm_b_eq_axb(a, b);

//     errcnt = 0;
//     for(int i=0; i<9; i++){
//         if(tmp[i] != b[i]){
//             errcnt++;
//             printf("i=%d ref=%lf myans=%lf\n", i, tmp[i], b[i]);
//         }
//     }
//     printf("errcnt = %d time_elapsed_avg = %.2lf ns ( ~ %.0lf ticks ), %lf Gflops\n",
//         errcnt,
//         time_elapsed * 1e9 / bench_times,
//         (time_elapsed * 1e9 / bench_times)*2.0,
//         (2 * 27.0) / (time_elapsed * 1e3 / bench_times) / 1e6
//     );

//     free(a);
//     free(b);
//     free(c);
//     free(tmp);
// }

// int analyzer_blas_check()
// {
//     double *a = malloc(sizeof(double) * 9);

//     int errcnt = 0;

//     for (int i = 0; i < 9; i++)
//     {
//         a[i] = (i + 3) / (5.0);
//     }

//     int bench_times = 100000000;
//     struct timeval time_start;
//     analyzer_timer_start(&time_start);
//     for (int i = 0; i < bench_times; i++)
//     {
//         dblas_3x3_inv(a);
//     }
//     double time_elapsed = analyzer_timer_stop(&time_start);

//     a[0] = 1; a[1] = 1; a[2] = 1;
//     a[3] = 1; a[4] = 2; a[5] = 2;
//     a[6] = 1; a[7] = 2; a[8] = 3;

//     // a[0] = 1; a[1] = 1; a[2] = 1;
//     // a[3] = 0; a[4] = 1; a[5] = 1;
//     // a[6] = 0; a[7] = 0; a[8] = 1;

//     // a[0] = 1;
//     // a[1] = 0;
//     // a[2] = 0;
//     // a[3] = 0;
//     // a[4] = 1;
//     // a[5] = 0;
//     // a[6] = 0;
//     // a[7] = 0;
//     // a[8] = 1;

//     dblas_3x3_inv(a);

//     for (int i = 0; i < 9; i++)
//     {
//         printf("a[%d] = %lf\n", i, a[i]);
//     }

//     printf("time_elapsed_avg = %.2lf ns ( ~ %.0lf ticks )\n",
//            time_elapsed * 1e9 / bench_times,
//            (time_elapsed * 1e9 / bench_times) * 2.0);

//     free(a);
// }